/**
* This file is part of ORB-SLAM2.
*
* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
* For more information see <https://github.com/raulmur/ORB_SLAM2>
*
* ORB-SLAM2 is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM2 is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
*/

#include "Viewer.h"
#include <pangolin/pangolin.h>

#include <mutex>

namespace ORB_SLAM2
{

Viewer::Viewer(System* pSystem, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Tracking *pTracking, const string &strSettingPath):
    mpSystem(pSystem), mpFrameDrawer(pFrameDrawer),mpMapDrawer(pMapDrawer), mpTracker(pTracking),
    mbFinishRequested(false), mbFinished(true), mbStopped(true), mbStopRequested(false)
{
    cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);

    float fps = fSettings["Camera.fps"];
    if(fps<1)
        fps=30;
    mT = 1e3/fps;

    mImageWidth = fSettings["Camera.width"];
    mImageHeight = fSettings["Camera.height"];
    if(mImageWidth<1 || mImageHeight<1)
    {
        mImageWidth = 640;
        mImageHeight = 480;
    }

    mViewpointX = fSettings["Viewer.ViewpointX"];
    mViewpointY = fSettings["Viewer.ViewpointY"];
    mViewpointZ = fSettings["Viewer.ViewpointZ"];
    mViewpointF = fSettings["Viewer.ViewpointF"];
}

//将cv::mat类型数据转化成pangolin的unsigned char型数组
void Viewer::setImageData(unsigned char * imageArray, cv::Mat image)
{
  //确定行数和列数
  int clos = image.cols;
  int rows = image.rows;
  
  //将每个像素的每个通道的值按照一维数组的形式放入imageArray
  int imageArray_count=0;
  for ( int i=0;i<rows;i++ )
  {
    for ( int j=0;j<clos;j++ )
    {
      imageArray[imageArray_count] = (unsigned char)image.at<cv::Vec3b>(i,j)[0];
      imageArray_count++;
      imageArray[imageArray_count] = (unsigned char)image.at<cv::Vec3b>(i,j)[1];
      imageArray_count++;
      imageArray[imageArray_count] = (unsigned char)image.at<cv::Vec3b>(i,j)[2];
      imageArray_count++;
    }
  }
}

void Viewer::Run()
{
    mbFinished = false;
    mbStopped = false;

    pangolin::CreateWindowAndBind("ORB-SLAM2: Map Viewer",1024,768);

    // 3D Mouse handler requires depth testing to be enabled
    glEnable(GL_DEPTH_TEST);

    // Issue specific OpenGl we might need
    glEnable (GL_BLEND);
    glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    //2018.5.22
    pangolin::CreatePanel("menu").SetBounds(1,pangolin::Attach::Pix(500),0.0,pangolin::Attach::Pix(175)); 
    //pangolin::CreatePanel("menu").SetBounds(0.0,1.0,0.0,pangolin::Attach::Pix(175));
    pangolin::Var<bool> menuFollowCamera("menu.Follow Camera",true,true);
    pangolin::Var<bool> menuShowPoints("menu.Show Points",true,true);
    pangolin::Var<bool> menuShowKeyFrames("menu.Show KeyFrames",true,true);
    pangolin::Var<bool> menuShowGraph("menu.Show Graph",true,true);
    pangolin::Var<bool> menuLocalizationMode("menu.Localization Mode",false,true);
    pangolin::Var<bool> menuReset("menu.Reset",false,false);

    // Define Camera Render Object (for view / scene browsing)
    pangolin::OpenGlRenderState s_cam(
                pangolin::ProjectionMatrix(1024,768,mViewpointF,mViewpointF,512,389,0.1,1000),
                pangolin::ModelViewLookAt(mViewpointX,mViewpointY,mViewpointZ, 0,0,0,0.0,-1.0, 0.0)
                );

    // Add named OpenGL viewport to window and provide 3D Handler
    pangolin::View& d_cam = pangolin::CreateDisplay()
            .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f/768.0f)
            .SetHandler(new pangolin::Handler3D(s_cam));
    
    //2018.5.22
    //图像显示“窗口”，位置与按键栏紧邻，大小640*480
    pangolin::View& d_image = pangolin::Display("image")
      .SetBounds(1,pangolin::Attach::Pix(500),pangolin::Attach::Pix(175),pangolin::Attach::Pix(175+290),640.0f/480.0f)
      .SetLock(pangolin::LockRight, pangolin::LockBottom);
	    
    pangolin::OpenGlMatrix Twc;
    Twc.SetIdentity();

    //2018.5.22
    //cv::namedWindow("ORB-SLAM2: Current Frame");

    bool bFollow = true;
    bool bLocalizationMode = false;

    while(1)
    {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        mpMapDrawer->GetCurrentOpenGLCameraMatrix(Twc); // 当前帧 位姿 OpenGL Matrix

        if(menuFollowCamera && bFollow)
        {
            s_cam.Follow(Twc); // 相机视角跟随
        }
        else if(menuFollowCamera && !bFollow)
        {
            s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(mViewpointX, mViewpointY, mViewpointZ, 0,0,0,0.0,-1.0, 0.0));
            s_cam.Follow(Twc);
            bFollow = true; // 防止一直循环
        }
        else if(!menuFollowCamera && bFollow)
        {
            bFollow = false;
        }

        if(menuLocalizationMode && !bLocalizationMode)
        {
            mpSystem->ActivateLocalizationMode(); // 仅定位模式
            bLocalizationMode = true;
        }
        else if(!menuLocalizationMode && bLocalizationMode)
        {
            mpSystem->DeactivateLocalizationMode(); // 定位加建图
            bLocalizationMode = false;
        }

        d_cam.Activate(s_cam);
        glClearColor(1.0f,1.0f,1.0f,1.0f);
        
        // 在xyz平面上显示网格
        // mpMapDrawer->DrawGrid();
        mpMapDrawer->DrawCurrentCamera(Twc);  // 画当前相机

        //========== 显示 ORB-SLAM2 地图点 / 稠密octo-map点 ========================
        if(menuShowPoints)
            mpMapDrawer->DrawMapPoints();// 显示  ORB-SLAM2  地图点
        else
        { // octomap 地图显示
            mpMapDrawer->DrawOctoMap();// 显示 octomap 
        }

        //2018.5.22
        cv::Mat im = mpFrameDrawer->DrawFrame();
        // cv::imshow("ORB-SLAM2: Current Frame",im);
        // cv::waitKey(mT);
        int width =  im.cols;
        int height = im.rows;
        // pangolin的输入图像数组
        unsigned char* imageArray = new unsigned char[3*width*height];
        pangolin::GlTexture imageTexture(width,height);	
        setImageData(imageArray,im);
        imageTexture.Upload(imageArray,GL_RGB,GL_UNSIGNED_BYTE);
        d_image.Activate();
        glColor3f(1.0,1.0,1.0);
        // revert image to show
        imageTexture.RenderToViewportFlipY();
        delete[] imageArray;
        
        pangolin::FinishFrame();

        if(menuReset)
        {
            menuShowGraph = true;
            menuShowKeyFrames = true;
            menuShowPoints = true;
            menuLocalizationMode = false;
            if(bLocalizationMode)
                mpSystem->DeactivateLocalizationMode();
            bLocalizationMode = false;
            bFollow = true;
            menuFollowCamera = true;
            mpSystem->Reset();
            menuReset = false;
        }

        // if(menuSave) // 保存 地图==============
        // {
        //    mpSystem->SaveMap("map.bin"); // orb-slam2 保存系统地图，便于重启后载入地图，重定位
        //    mpMapDrawer->SaveOctoMap("octomap.ot");// 保存octomap地图
        //    menuSave = false;
        //    cout<<"save done!"<<endl;
        // }

        if(Stop())
        {
            while(isStopped())
            {
                usleep(3000);
            }
        }

        if(CheckFinish())
            break;
    }

    SetFinish();
}

void Viewer::RequestFinish()
{
    unique_lock<mutex> lock(mMutexFinish);
    mbFinishRequested = true;
}

bool Viewer::CheckFinish()
{
    unique_lock<mutex> lock(mMutexFinish);
    return mbFinishRequested;
}

void Viewer::SetFinish()
{
    unique_lock<mutex> lock(mMutexFinish);
    mbFinished = true;
}

bool Viewer::isFinished()
{
    unique_lock<mutex> lock(mMutexFinish);
    return mbFinished;
}

void Viewer::RequestStop()
{
    unique_lock<mutex> lock(mMutexStop);
    if(!mbStopped)
        mbStopRequested = true;
}

bool Viewer::isStopped()
{
    unique_lock<mutex> lock(mMutexStop);
    return mbStopped;
}

bool Viewer::Stop()
{
    unique_lock<mutex> lock(mMutexStop);
    unique_lock<mutex> lock2(mMutexFinish);

    if(mbFinishRequested)
        return false;
    else if(mbStopRequested)
    {
        mbStopped = true;
        mbStopRequested = false;
        return true;
    }

    return false;

}

void Viewer::Release()
{
    unique_lock<mutex> lock(mMutexStop);
    mbStopped = false;
}

}
